/*
*  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*  1. Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*
*  2. Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*
*  3. Neither the name of the copyright holder nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  move_base_action.cpp
*
*  authors:
*    Sebastian Pütz <spuetz@uni-osnabrueck.de>
*    Jorge Santos Simón <santos@magazino.eu>
*
*/

#include <limits>

#include <mbf_utility/navigation_utility.h>

#include "mbf_abstract_nav/MoveBaseFlexConfig.h"
#include "mbf_abstract_nav/move_base_action.h"

namespace mbf_abstract_nav
{
MoveBaseAction::MoveBaseAction(const std::string &name, const mbf_utility::RobotInformation &robot_info,
                               const std::vector<std::string> &behaviors)
        : name_(name), robot_info_(robot_info), private_nh_("~"), action_client_exe_path_(private_nh_, "exe_path"),
          action_client_get_path_(private_nh_, "get_path"), action_client_recovery_(private_nh_, "recovery"),
          oscillation_timeout_(0), oscillation_distance_(0), recovery_enabled_(true), behaviors_(behaviors),
          action_state_(NONE), recovery_trigger_(NONE), dist_to_goal_(std::numeric_limits<double>::infinity()),
          replanning_thread_(boost::bind(&MoveBaseAction::replanningThread, this)) {}

MoveBaseAction::~MoveBaseAction()
{
    action_state_ = NONE;
    replanning_thread_.join();
}

void MoveBaseAction::reconfigure(
        mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
{
    if (config.planner_frequency > 0.0)
        replanning_period_.fromSec(1.0 / config.planner_frequency);
    else replanning_period_.fromSec(0.0);
    oscillation_timeout_ = ros::Duration(config.oscillation_timeout);
    oscillation_distance_ = config.oscillation_distance;
    recovery_enabled_ = config.recovery_enabled;
}

void MoveBaseAction::cancel()
{
    action_state_ = CANCELED;
    if (!action_client_get_path_.getState().isDone())
    {
        action_client_get_path_.cancelGoal();
    }
    if (!action_client_exe_path_.getState().isDone())
    {
        action_client_exe_path_.cancelGoal();
    }
    if (!action_client_recovery_.getState().isDone())
    {
        action_client_recovery_.cancelGoal();
    }
}

void MoveBaseAction::start(GoalHandle &goal_handle)
{
    dist_to_goal_ = std::numeric_limits<double>::infinity();
    action_state_ = GET_PATH;
    goal_handle.setAccepted();
    goal_handle_ = goal_handle;
    ROS_DEBUG_STREAM_NAMED("move_base", "Start action \"move_base\"");
    const mbf_msgs::MoveBaseGoal &goal = *goal_handle.getGoal();
    mbf_msgs::MoveBaseResult move_base_result;
    get_path_goal_.target_pose = goal.target_pose;
    get_path_goal_.use_start_pose = false; // use the robot pose
    get_path_goal_.planner = goal.planner;
    exe_path_goal_.controller = goal.controller;
    ros::Duration connection_timeout(1.0);
    last_oscillation_reset_ = ros::Time::now();
    // start recovering with the first behavior, use the recovery behaviors from the action request, if specified,
    // otherwise, use all loaded behaviors.
    recovery_behaviors_ = goal.recovery_behaviors.empty() ? behaviors_ : goal.recovery_behaviors;
    current_recovery_behavior_ = recovery_behaviors_.begin();
    // get the current robot pose only at the beginning, as exe_path will keep updating it as we move
    if (!robot_info_.getRobotPose(robot_pose_))
    {
        ROS_ERROR_STREAM_NAMED("move_base", "Could not get the current robot pose!");
        move_base_result.message = "Could not get the current robot pose!";
        move_base_result.outcome = mbf_msgs::MoveBaseResult::TF_ERROR;
        goal_handle.setAborted(move_base_result, move_base_result.message);
        return;
    }
    goal_pose_ = goal.target_pose;
    // wait for server connections
    if (!action_client_get_path_.waitForServer(connection_timeout) ||
        !action_client_exe_path_.waitForServer(connection_timeout) ||
        !action_client_recovery_.waitForServer(connection_timeout))
    {
        ROS_ERROR_STREAM_NAMED("move_base", "Could not connect to one or more of move_base_flex actions: "
                                            "\"get_path\", \"exe_path\", \"recovery \"!");
        move_base_result.outcome = mbf_msgs::MoveBaseResult::INTERNAL_ERROR;
        move_base_result.message = "Could not connect to the move_base_flex actions!";
        goal_handle.setAborted(move_base_result, move_base_result.message);
        return;
    }
    // call get_path action server to get a first plan
    action_client_get_path_.sendGoal(
            get_path_goal_, boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2));
}

void MoveBaseAction::actionExePathActive()
{
    ROS_DEBUG_STREAM_NAMED("move_base", "The \"exe_path\" action is active.");
}

void MoveBaseAction::actionExePathFeedback(
        const mbf_msgs::ExePathFeedbackConstPtr &feedback)
{
    mbf_msgs::MoveBaseFeedback move_base_feedback;
    move_base_feedback.outcome = feedback->outcome;
    move_base_feedback.message = feedback->message;
    move_base_feedback.angle_to_goal = feedback->angle_to_goal;
    move_base_feedback.dist_to_goal = feedback->dist_to_goal;
    move_base_feedback.current_pose = feedback->current_pose;
    move_base_feedback.last_cmd_vel = feedback->last_cmd_vel;
    goal_handle_.publishFeedback(move_base_feedback);
    dist_to_goal_ = feedback->dist_to_goal;
    robot_pose_ = feedback->current_pose;
    // we create a navigation-level oscillation detection using exe_path action's feedback,
    // as the later doesn't handle oscillations created by quickly failing repeated plans
    // if oscillation detection is enabled by oscillation_timeout != 0
    if (!oscillation_timeout_.isZero())
    {
        // check if oscillating
        // moved more than the minimum oscillation distance
        if (mbf_utility::distance(robot_pose_, last_oscillation_pose_) >= oscillation_distance_)
        {
            last_oscillation_reset_ = ros::Time::now();
            last_oscillation_pose_ = robot_pose_;
            if (recovery_trigger_ == OSCILLATING)
            {
                ROS_INFO_NAMED("move_base", "Recovered from robot oscillation: restart recovery behaviors");
                current_recovery_behavior_ = recovery_behaviors_.begin();
                recovery_trigger_ = NONE;
            }
        }
        else if (last_oscillation_reset_ + oscillation_timeout_ < ros::Time::now())
        {
            std::stringstream oscillation_msgs;
            oscillation_msgs << "Robot is oscillating for " << (ros::Time::now() - last_oscillation_reset_).toSec() << "s!";
            ROS_WARN_STREAM_NAMED("move_base", oscillation_msgs.str());
            action_client_exe_path_.cancelGoal();
            if (attemptRecovery()) recovery_trigger_ = OSCILLATING;
            else
            {
                mbf_msgs::MoveBaseResult move_base_result;
                move_base_result.outcome = mbf_msgs::MoveBaseResult::OSCILLATION;
                move_base_result.message = oscillation_msgs.str();
                move_base_result.final_pose = robot_pose_;
                move_base_result.angle_to_goal = move_base_feedback.angle_to_goal;
                move_base_result.dist_to_goal = move_base_feedback.dist_to_goal;
                goal_handle_.setAborted(move_base_result, move_base_result.message);
            }
        }
    }
}

void MoveBaseAction::actionGetPathDone(
        const actionlib::SimpleClientGoalState &state,
        const mbf_msgs::GetPathResultConstPtr &result_ptr)
{
    const mbf_msgs::GetPathResult &get_path_result = *result_ptr;
    mbf_msgs::MoveBaseResult move_base_result;
    // copy result from get_path action
    fillMoveBaseResult(get_path_result, move_base_result);
    switch (state.state_)
    {
        case actionlib::SimpleClientGoalState::PENDING:
            ROS_FATAL_STREAM_NAMED("move_base", "get_path PENDING state not implemented, this should not be reachable!");
            break;

        case actionlib::SimpleClientGoalState::SUCCEEDED:
            ROS_DEBUG_STREAM_NAMED("move_base", "Action \" move_base\" received a path from \" get_path\": " << state.getText());
            exe_path_goal_.path = get_path_result.path;
            ROS_DEBUG_STREAM_NAMED("move_base", "Action \" move_base\" sends the path to \" exe_path\".");
            if (recovery_trigger_ == GET_PATH)
            {
                ROS_WARN_NAMED("move_base", "Recovered from planner failure: restart recovery behaviors");
                current_recovery_behavior_ = recovery_behaviors_.begin();
                recovery_trigger_ = NONE;
            }
            action_client_exe_path_.sendGoal(
                    exe_path_goal_,
                    boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
                    boost::bind(&MoveBaseAction::actionExePathActive, this),
                    boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
            action_state_ = EXE_PATH;
            break;

        case actionlib::SimpleClientGoalState::ABORTED:
            if (attemptRecovery()) recovery_trigger_ = GET_PATH;
            else
            {
                // copy result from get_path action
                ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the planner: " << get_path_result.message);
                goal_handle_.setAborted(move_base_result, state.getText());
            }
            action_state_ = FAILED;
            break;

        case actionlib::SimpleClientGoalState::RECALLED:
        case actionlib::SimpleClientGoalState::PREEMPTED:
            ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"get_path\" has been " << state.toString());
            if (action_state_ == CANCELED)
            {
                // move_base preempted while executing get_path; fill result and report canceled to the client
                ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing get_path");
                goal_handle_.setCanceled(move_base_result, state.getText());
            }
            break;

        case actionlib::SimpleClientGoalState::REJECTED:
            ROS_ERROR_STREAM_NAMED("move_base", "The last action goal to \"get_path\" has been " << state.toString());
            goal_handle_.setCanceled(move_base_result, state.getText());
            action_state_ = FAILED;
            break;

        case actionlib::SimpleClientGoalState::LOST:
            ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"get_path\"!");
            goal_handle_.setAborted();
            action_state_ = FAILED;
            break;

        default:
            ROS_FATAL_STREAM_NAMED("move_base", "Reached unknown action server state!");
            goal_handle_.setAborted();
            action_state_ = FAILED;
            break;
    }
}

void MoveBaseAction::actionExePathDone(
        const actionlib::SimpleClientGoalState &state,
        const mbf_msgs::ExePathResultConstPtr &result_ptr)
{
    ROS_DEBUG_STREAM_NAMED("move_base", "Action \"exe_path\" finished.");
    const mbf_msgs::ExePathResult &exe_path_result = *result_ptr;
    mbf_msgs::MoveBaseResult move_base_result;
    // copy result from exe_path action
    fillMoveBaseResult(exe_path_result, move_base_result);
    ROS_DEBUG_STREAM_NAMED("move_base", "Current state: " << state.toString());
    switch (state.state_)
    {
        case actionlib::SimpleClientGoalState::SUCCEEDED:
            move_base_result.outcome = mbf_msgs::MoveBaseResult::SUCCESS;
            move_base_result.message = "Action \"move_base\" succeeded!";
            ROS_INFO_STREAM_NAMED("move_base", move_base_result.message);
            goal_handle_.setSucceeded(move_base_result, move_base_result.message);
            action_state_ = SUCCEEDED;
            break;

        case actionlib::SimpleClientGoalState::ABORTED:
            action_state_ = FAILED;
            switch (exe_path_result.outcome)
            {
                case mbf_msgs::ExePathResult::INVALID_PATH:
                case mbf_msgs::ExePathResult::TF_ERROR:
                case mbf_msgs::ExePathResult::NOT_INITIALIZED:
                case mbf_msgs::ExePathResult::INVALID_PLUGIN:
                case mbf_msgs::ExePathResult::INTERNAL_ERROR:
                    // none of these errors is recoverable
                    goal_handle_.setAborted(move_base_result, state.getText());
                    break;

                default:
                    // all the rest are, so we start calling the recovery behaviors in sequence
                    if (attemptRecovery()) recovery_trigger_ = EXE_PATH;
                    else
                    {
                        ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the controller: " << exe_path_result.message);
                        goal_handle_.setAborted(move_base_result, state.getText());
                    }
                    break;
            }
            break;

        case actionlib::SimpleClientGoalState::RECALLED:
        case actionlib::SimpleClientGoalState::PREEMPTED:
            ROS_INFO_STREAM_NAMED("move_base",
                                  "The last action goal to \"exe_path\" has been " << state.toString());
            if (action_state_ == CANCELED)
            {
                // move_base preempted while executing exe_path; fill result and report canceled to the client
                ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing exe_path");
                goal_handle_.setCanceled(move_base_result, state.getText());
            }
            break;

        case actionlib::SimpleClientGoalState::REJECTED:
            ROS_ERROR_STREAM_NAMED("move_base", "The last action goal to \"exe_path\" has been " << state.toString());
            goal_handle_.setCanceled(move_base_result, state.getText());
            action_state_ = FAILED;
            break;

        case actionlib::SimpleClientGoalState::LOST:
            ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"exe_path\"!");
            goal_handle_.setAborted();
            action_state_ = FAILED;
            break;

        default:
            ROS_FATAL_STREAM_NAMED("move_base", "Reached unreachable case! Unknown SimpleActionServer state!");
            goal_handle_.setAborted();
            action_state_ = FAILED;
            break;
    }
}

bool MoveBaseAction::attemptRecovery()
{
    if (!recovery_enabled_)
    {
        ROS_WARN_STREAM_NAMED("move_base", "Recovery behaviors are disabled!");
        return false;
    }
    if (current_recovery_behavior_ == recovery_behaviors_.end())
    {
        if (recovery_behaviors_.empty()) ROS_WARN_STREAM_NAMED("move_base", "No Recovery Behaviors loaded!");
        else ROS_WARN_STREAM_NAMED("move_base", "Executed all available recovery behaviors!");
        return false;
    }
    recovery_goal_.behavior = *current_recovery_behavior_;
    ROS_DEBUG_STREAM_NAMED("move_base", "Start recovery behavior\""
            << *current_recovery_behavior_ << "\".");
    action_client_recovery_.sendGoal(recovery_goal_, boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2));
    action_state_ = RECOVERY;
    return true;
}

void MoveBaseAction::actionRecoveryDone(
        const actionlib::SimpleClientGoalState &state,
        const mbf_msgs::RecoveryResultConstPtr &result_ptr)
{
    // give the robot some time to stop oscillating after executing the recovery behavior
    last_oscillation_reset_ = ros::Time::now();
    const mbf_msgs::RecoveryResult &recovery_result = *result_ptr;
    mbf_msgs::MoveBaseResult move_base_result;
    // copy result from recovery action
    fillMoveBaseResult(recovery_result, move_base_result);
    switch (state.state_)
    {
        case actionlib::SimpleClientGoalState::REJECTED:
        case actionlib::SimpleClientGoalState::ABORTED:
            action_state_ = FAILED;
            ROS_DEBUG_STREAM_NAMED("move_base", "The recovery behavior \"" << *current_recovery_behavior_ << "\" has failed. ");
            ROS_DEBUG_STREAM("Recovery behavior message: " << recovery_result.message << ", outcome: " << recovery_result.outcome);
            current_recovery_behavior_++; // use next behavior;
            if (current_recovery_behavior_ == recovery_behaviors_.end())
            {
                ROS_DEBUG_STREAM_NAMED("move_base", "All recovery behaviors failed. Abort recovering and abort the move_base action");
                goal_handle_.setAborted(move_base_result, "All recovery behaviors failed.");
            }
            else
            {
                recovery_goal_.behavior = *current_recovery_behavior_;
                ROS_INFO_STREAM_NAMED("move_base", "Run the next recovery behavior \""
                        << *current_recovery_behavior_ << "\".");
                action_client_recovery_.sendGoal(recovery_goal_, boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2));
            }
            break;
        case actionlib::SimpleClientGoalState::SUCCEEDED:
            //go to planning state
            ROS_DEBUG_STREAM_NAMED("move_base", "Execution of the recovery behavior \""
                    << *current_recovery_behavior_ << "\" succeeded!");
            ROS_DEBUG_STREAM_NAMED("move_base", "Try planning again and increment the current recovery behavior in the list.");
            action_state_ = GET_PATH;
            current_recovery_behavior_++; // use next behavior, the next time;
            action_client_get_path_.sendGoal(get_path_goal_, boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2));
            break;
        case actionlib::SimpleClientGoalState::RECALLED:
        case actionlib::SimpleClientGoalState::PREEMPTED:
            ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"recovery\" has been preempted!");
            if (action_state_ == CANCELED)
            {
                // move_base preempted while executing a recovery; fill result and report canceled to the client
                ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing a recovery behavior");
                goal_handle_.setCanceled(move_base_result, state.getText());
            }
            break;

        case actionlib::SimpleClientGoalState::LOST:
            ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"recovery\"!");
            goal_handle_.setAborted();
            action_state_ = FAILED;
            break;

        default:
            ROS_FATAL_STREAM_NAMED("move_base", "Reached unreachable case! Unknown state!");
            goal_handle_.setAborted();
            action_state_ = FAILED;
            break;
    }
}

bool MoveBaseAction::replanningActive() const
{
    // replan only while following a path and if replanning is enabled (can be disabled by dynamic reconfigure)
    return !replanning_period_.isZero() && action_state_ == EXE_PATH && dist_to_goal_ > 0.1;
}

void MoveBaseAction::replanningThread()
{
    ros::Duration update_period(0.005);
    ros::Time last_replan_time(0.0);
    while (ros::ok())
    {
        if (!action_client_get_path_.getState().isDone())
        {
            if (action_client_get_path_.waitForResult(update_period))
            {
                actionlib::SimpleClientGoalState state = action_client_get_path_.getState();
                mbf_msgs::GetPathResultConstPtr result = action_client_get_path_.getResult();
                if (state == actionlib::SimpleClientGoalState::SUCCEEDED && replanningActive())
                {
                    ROS_DEBUG_STREAM_NAMED("move_base", "Replanning succeeded; sending a goal to \"exe_path\" with the new plan");
                    exe_path_goal_.path = result->path;
                    mbf_msgs::ExePathGoal goal(exe_path_goal_);
                    action_client_exe_path_.sendGoal(goal,
                                                     boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
                                                     boost::bind(&MoveBaseAction::actionExePathActive, this),
                                                     boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
                }
                else
                {
                    ROS_DEBUG_STREAM_NAMED("move_base",
                            "Replanning failed with error code " << result->outcome << ": " << result->message);
                }
            }
            // else keep waiting for planning to complete (we already waited update_period in waitForResult)
        }
        else if (!replanningActive()) update_period.sleep();
        else if (ros::Time::now() - last_replan_time >= replanning_period_)
        {
            ROS_DEBUG_STREAM_NAMED("move_base", "Next replanning cycle, using the \"get_path\" action!");
            action_client_get_path_.sendGoal(get_path_goal_);
            last_replan_time = ros::Time::now();
        }
    }
}

} /* namespace mbf_abstract_nav */
